///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_CV_COCCUPANCYOCTREE_H_
#define _BUOLA_CV_COCCUPANCYOCTREE_H_

#include <buola/cv/coctree.h>
#include <buola/image/format.h>
#include <unordered_set>

namespace buola { namespace cvi {

class COccupancyOcTree : public COcTree<float>
{
public:
    typedef COcTree<float> TBase;
    typedef TBase::TKey TKey;
    typedef TBase::TNode TNode;

    enum EOccupancy
    {
        OCCUPIED,
        FREESPACE,
        MIXED,
        UNKNOWN
    };
    
    explicit COccupancyOcTree(float pResolution=1.0f)
        :   COcTree<float>(pResolution)
    {
        mClampMin=-2; //0.1192 probability
        mClampMax=3.5; //0.971 probability
        mProbThres=0; //0.5 probability
        mProbHit=0.85; //0.7 probability
        mProbMiss=-0.4; //0.4 probability
    }

    ~COccupancyOcTree()
    {}

    template<typename tCloud>
    void InsertCloud(const tCloud &pCloud,float pMaxRange=-1.0f);

    void UpdateNode(const TKey &pKey,bool pOccupied)
    {
        auto &lNode=this->GetNode(pKey);

        if(!lNode.HasData())
            lNode.SetValue(0.0);

        if(pOccupied)
        {
            if(lNode.Value()>=mClampMax)
                return;
            lNode.Value()+=mProbHit;
            if(lNode.Value()>=mClampMax)
                lNode.Value()=mClampMax;
        }
        else
        {
            if(lNode.Value()<=mClampMin)
                return;
            lNode.Value()+=mProbMiss;
            if(lNode.Value()<=mClampMin)
                lNode.Value()=mClampMin;
        }
    }

    // occupancy parameters of tree, stored in logodds:
    float mClampMin;
    float mClampMax;
    float mProbHit;
    float mProbMiss;
    float mProbThres;
};

template<typename tCloud>
void COccupancyOcTree::InsertCloud(const tCloud &pCloud,float pMaxRange)
{
    std::unordered_set<TKey> lOccupied,lFree;

    for(auto &p : pCloud)
    {
        mat::CVec3f lPoint=p.mPoint;
        if(pMaxRange<0.0f||norm(lPoint-pCloud.Origin())<=pMaxRange)
        {
            lOccupied.insert(ComputeKey(p.mPoint));
        }
        else
        {
            mat::CVec3f lDirection=lPoint-pCloud.Origin();
            lPoint=pCloud.Origin()+(lDirection*(pMaxRange/norm(lDirection)));
        }
        
        auto lKeys=this->ComputeRayKeys(pCloud.Origin(),p.mPoint);
        lFree.insert(lKeys.begin(),lKeys.end());
    }

    msg_info() << lOccupied.size() << "," << lFree.size() << "\n";

    start_timer();
    for(auto &lKey : lOccupied)
        lFree.erase(lKey);
    end_timer();
   
    for(auto &lKey : lFree)
        UpdateNode(lKey,false);

    for(auto &lKey : lOccupied)
        UpdateNode(lKey,true);
}

img::CImage_gray8 get_height_map(const COccupancyOcTree &pTree,const CSize_i &pSize,float pTableZ);
mat::CMat_f get_height_map_mat(const COccupancyOcTree &pTree,const CSize_i &pSize,
                                     float pMinX,float pMaxX,float pMinY,float pMaxY,float pMinZ,float pMaxZ);

/*namespace cvi*/ } /*namespace buola*/ }

#endif






#if 0

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::updateInnerOccupancy(){
    this->updateInnerOccupancyRecurs(this->itsRoot, 0);
  }

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::updateInnerOccupancyRecurs(NODE* node, unsigned int depth){
    // only recurse and update for inner nodes:
    if (node->hasChildren()){
      // return early for last level:
      if (depth < this->tree_depth){
        for (unsigned int i=0; i<8; i++) {
          if (node->childExists(i)) {
            updateInnerOccupancyRecurs(node->getChild(i), depth+1);
          }
        }
      }
      node->updateOccupancyChildren();
    }
  }

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::toMaxLikelihood() {

    // convert bottom up
    for (unsigned int depth=this->tree_depth; depth>0; depth--) {
      toMaxLikelihoodRecurs(this->itsRoot, 0, depth);
    }

    // convert root
    nodeToMaxLikelihood(this->itsRoot);
  }

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::toMaxLikelihoodRecurs(NODE* node, unsigned int depth,
      unsigned int max_depth) {

    if (depth < max_depth) {
      for (unsigned int i=0; i<8; i++) {
        if (node->childExists(i)) {
          toMaxLikelihoodRecurs(node->getChild(i), depth+1, max_depth);
        }
      }
    }

    else { // max level reached
      nodeToMaxLikelihood(node);
    }
  }

  template <class NODE>
  bool OccupancyOcTreeBase<NODE>::castRay(const point3d& origin, const point3d& directionP, point3d& end,
                                          bool ignoreUnknown, double maxRange) const {

    /// ----------  see OcTreeBase::computeRayKeys  -----------

    // Initialization phase -------------------------------------------------------
    OcTreeKey current_key;
    if ( !OcTreeBase<NODE>::genKey(origin, current_key) ) {
      OCTOMAP_WARNING_STR("Coordinates out of bounds during ray casting");
      return false;
    }

    NODE* startingNode = this->search(current_key);
    if (startingNode){
      if (isNodeOccupied(startingNode)){
        // Occupied node found at origin
        // (need to convert from key, since origin does not need to be a voxel center)
        genCoords(current_key, this->tree_depth, end);
        return true;
      }
    } else if(!ignoreUnknown){
      OCTOMAP_ERROR_STR("Origin node at " << origin << " for raycasting not found, does the node exist?");
      genCoords(current_key, this->tree_depth, end);
      return false;
    }

    point3d direction = directionP.normalized();
    bool max_range_set = (maxRange > 0.);

    int step[3];
    double tMax[3];
    double tDelta[3];

    for(unsigned int i=0; i < 3; ++i) {
      // compute step direction
      if (direction(i) > 0.0) step[i] =  1;
      else if (direction(i) < 0.0)   step[i] = -1;
      else step[i] = 0;

      // compute tMax, tDelta
      if (step[i] != 0) {
        // corner point of voxel (in direction of ray)
        float voxelBorder(0);
        this->genCoordFromKey(current_key[i], voxelBorder);
        voxelBorder += float (step[i] * this->resolution * 0.5);

        tMax[i] = ( voxelBorder - origin(i) ) / direction(i);
        tDelta[i] = this->resolution / fabs( direction(i) );
      }
      else {
        tMax[i] =  std::numeric_limits<double>::max();
        tDelta[i] = std::numeric_limits<double>::max();
      }
    }

    if (step[0] == 0 && step[1] == 0 && step[2] == 0){
        OCTOMAP_ERROR("Raycasting in direction (0,0,0) is not possible!");
        return false;
    }

    // for speedup:
    point3d origin_scaled = origin;
    origin_scaled /= (float) this->resolution;
    double maxrange_2 = maxRange / this->resolution;  // scale
    maxrange_2 = maxrange_2*maxrange_2; // squared dist
    double res_2 = this->resolution/2.;
    // Incremental phase  ---------------------------------------------------------

    bool done = false;

    while (!done) {
      unsigned int dim;

      // find minimum tMax:
      if (tMax[0] < tMax[1]){
        if (tMax[0] < tMax[2]) dim = 0;
        else                   dim = 2;
      }
      else {
        if (tMax[1] < tMax[2]) dim = 1;
        else                   dim = 2;
      }

      // check for overflow:
      if ((step[dim] < 0 && current_key[dim] == 0)
              || (step[dim] > 0 && current_key[dim] == 2* this->tree_max_val-1))
      {
          OCTOMAP_WARNING("Coordinate hit bounds in dim %d, aborting raycast\n", dim);
          // return border point nevertheless:
          genCoords(current_key, this->tree_depth, end);
          return false;
      }

      // advance in direction "dim"
      current_key[dim] += step[dim];
      tMax[dim] += tDelta[dim];


      // generate world coords from key
      double dist_from_origin(0);
      for (unsigned int j = 0; j < 3; j++) {
        double coord = (double) current_key[j] - (double) this->tree_max_val + res_2; // center of voxel
        dist_from_origin += (coord - origin_scaled(j)) * (coord - origin_scaled(j));
        end(j) = (float) (coord * this->resolution);
      }

      if (max_range_set && (dist_from_origin > maxrange_2) ) { // reached user specified maxrange
        return false;
      }

      NODE* currentNode = this->search(current_key);
      if (currentNode){
        if (isNodeOccupied(currentNode)) {
          done = true;
          break;
        }
        // otherwise: node is free and valid, raycasting continues
      }

      else if (!ignoreUnknown){ // no node found, this usually means we are in "unknown" areas
        OCTOMAP_WARNING_STR("Search failed in OcTree::castRay() => an unknown area was hit in the map: " << end);
        return false;
      }
    } // end while

    return true;
  }


  template <class NODE> inline bool
  OccupancyOcTreeBase<NODE>::integrateMissOnRay(const point3d& origin, const point3d& end) {

    if (!this->computeRayKeys(origin, end, this->keyray)) {
      return false;
    }

    for(KeyRay::iterator it=this->keyray.begin(); it != this->keyray.end(); it++) {
      updateNode(*it, false); // insert freespace measurement
    }

    return true;
  }

  template <class NODE> bool
  OccupancyOcTreeBase<NODE>::insertRay(const point3d& origin, const point3d& end, double maxrange)
  {
    // cut ray at maxrange
    if ((maxrange > 0) && ((end - origin).norm () > maxrange))
      {
        point3d direction = (end - origin).normalized ();
        point3d new_end = origin + direction * (float) maxrange;
        return integrateMissOnRay(origin, new_end);
      }
    // insert complete ray
    else
      {
        if (!integrateMissOnRay(origin, end))
          return false;
        updateNode(end, true); // insert hit cell
        return true;
      }
  }

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getOccupied(point3d_list& node_centers, unsigned int max_depth) const {

    if (max_depth == 0)
      max_depth = this->tree_depth;

    for(typename OccupancyOcTreeBase<NODE>::leaf_iterator it = this->begin(max_depth),
        end=this->end(); it!= end; ++it)
    {
      if(this->isNodeOccupied(*it))
        node_centers.push_back(it.getCoordinate());
    }
  }


  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getOccupied(std::list<OcTreeVolume>& occupied_nodes, unsigned int max_depth) const{

    if (max_depth == 0)
      max_depth = this->tree_depth;

    for(typename OccupancyOcTreeBase<NODE>::leaf_iterator it = this->begin(max_depth),
            end=this->end(); it!= end; ++it)
    {
      if(this->isNodeOccupied(*it))
        occupied_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
    }

  }


  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getOccupied(std::list<OcTreeVolume>& binary_nodes,
                                              std::list<OcTreeVolume>& delta_nodes,
                                              unsigned int max_depth) const{
    if (max_depth == 0)
      max_depth = this->tree_depth;

    for(typename OccupancyOcTreeBase<NODE>::leaf_iterator it = this->begin(max_depth),
            end=this->end(); it!= end; ++it)
    {
      if(this->isNodeOccupied(*it)){
        if (it->getLogOdds() >= clampingThresMax)
          binary_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
        else
          delta_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
      }
    }
  }

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getFreespace(std::list<OcTreeVolume>& free_nodes, unsigned int max_depth) const{

    if (max_depth == 0)
      max_depth = this->tree_depth;

    for(typename OccupancyOcTreeBase<NODE>::leaf_iterator it = this->begin(max_depth),
            end=this->end(); it!= end; ++it)
    {
      if(!this->isNodeOccupied(*it))
        free_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
    }
  }


  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getFreespace(std::list<OcTreeVolume>& binary_nodes,
                                               std::list<OcTreeVolume>& delta_nodes,
                                               unsigned int max_depth) const{

    if (max_depth == 0)
      max_depth = this->tree_depth;

    for(typename OccupancyOcTreeBase<NODE>::leaf_iterator it = this->begin(max_depth),
            end=this->end(); it!= end; ++it)
    {
      if(!this->isNodeOccupied(*it)){
        if (it->getLogOdds() <= clampingThresMin)
          binary_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
        else
          delta_nodes.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
      }
    }
  }


  template <class NODE>
  void OccupancyOcTreeBase<NODE>::setBBXMin (point3d& min) {
    bbx_min = min;
    if (!this->genKey(bbx_min, bbx_min_key)) {
      OCTOMAP_ERROR("ERROR while generating bbx min key.\n");
    }
  }

  template <class NODE>
  void OccupancyOcTreeBase<NODE>::setBBXMax (point3d& max) {
    bbx_max = max;
    if (!this->genKey(bbx_max, bbx_max_key)) {
      OCTOMAP_ERROR("ERROR while generating bbx max key.\n");
    }
  }


  template <class NODE>
  bool OccupancyOcTreeBase<NODE>::inBBX(const point3d& p) const {
    return ((p.x() >= bbx_min.x()) && (p.y() >= bbx_min.y()) && (p.z() >= bbx_min.z()) &&
            (p.x() <= bbx_max.x()) && (p.y() <= bbx_max.y()) && (p.z() <= bbx_max.z()) );
  }


  template <class NODE>
  bool OccupancyOcTreeBase<NODE>::inBBX(const OcTreeKey& key) const {
    return ((key[0] >= bbx_min_key[0]) && (key[1] >= bbx_min_key[1]) && (key[2] >= bbx_min_key[2]) &&
            (key[0] <= bbx_max_key[0]) && (key[1] <= bbx_max_key[1]) && (key[2] <= bbx_max_key[2]) );
  }

  template <class NODE>
  point3d OccupancyOcTreeBase<NODE>::getBBXBounds () const {
    octomap::point3d obj_bounds = (bbx_max - bbx_min);
    obj_bounds /= 2.;
    return obj_bounds;
  }

  template <class NODE>
  point3d OccupancyOcTreeBase<NODE>::getBBXCenter () const {
    octomap::point3d obj_bounds = (bbx_max - bbx_min);
    obj_bounds /= 2.;
    return bbx_min + obj_bounds;
  }


  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getOccupiedLeafsBBX(point3d_list& node_centers, point3d min, point3d max) const {

    OcTreeKey root_key, min_key, max_key;
    root_key[0] = root_key[1] = root_key[2] = this->tree_max_val;
    if (!this->genKey(min, min_key)) return;
    if (!this->genKey(max, max_key)) return;
    getOccupiedLeafsBBXRecurs(node_centers, this->tree_depth, this->itsRoot, 0, root_key, min_key, max_key);
  }


  template <class NODE>
  void OccupancyOcTreeBase<NODE>::getOccupiedLeafsBBXRecurs( point3d_list& node_centers, unsigned int max_depth,
                                                             NODE* node, unsigned int depth, const OcTreeKey& parent_key,
                                                             const OcTreeKey& min, const OcTreeKey& max) const {
    if (depth == max_depth) { // max level reached
      if (isNodeOccupied(node)) {
        point3d p;
        this->genCoords(parent_key, depth, p);
        node_centers.push_back(p);
      }
    }

    if (!node->hasChildren()) return;

    unsigned short int center_offset_key = this->tree_max_val >> (depth + 1);

    OcTreeKey child_key;

    for (unsigned int i=0; i<8; ++i) {
      if (node->childExists(i)) {

        computeChildKey(i, center_offset_key, parent_key, child_key);

        // overlap of query bbx and child bbx?
        if (!(
              ( min[0] > (child_key[0] + center_offset_key)) ||
              ( max[0] < (child_key[0] - center_offset_key)) ||
              ( min[1] > (child_key[1] + center_offset_key)) ||
              ( max[1] < (child_key[1] - center_offset_key)) ||
              ( min[2] > (child_key[2] + center_offset_key)) ||
              ( max[2] < (child_key[2] - center_offset_key))
               )) {
          getOccupiedLeafsBBXRecurs(node_centers, max_depth, node->getChild(i), depth+1, child_key, min, max);
        }
      }
    }
  }

} // namespace

#endif
